/*
 * Utilities.cpp
 *
 *  Created on: Jun 16, 2011
 *      Author: fqhuy
 */

#include "Utilities.hpp"
#include "FunctionsListOld.hpp"
#include <iostream>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>

using namespace std;

int ShowConfig(MART* mart) {
	int i = 0;
	cout << "---------Parameters for MART-------- \n";
	cout << "Num_Camera=" << mart->Num_Camera << endl;
	cout << "Pixelsize=" << mart->Pixelsize << endl;
	cout << "Start_VoxelX=" << mart->Start_VoxelX << endl;
	cout << "End_VoxelX=" << mart->End_VoxelX << endl;
	cout << "Start_VoxelY=" << mart->Start_VoxelY << endl;
	cout << "End_VoxelY=" << mart->End_VoxelY << endl;
	cout << "Start_VoxelZ=" << mart->Start_VoxelZ << endl;
	cout << "End_VoxelZ=" << mart->End_VoxelZ << endl;
	cout << "VPScaleFactor=" << mart->VPScaleFactor << endl;
	cout << "Mu=" << mart->Mu << endl;
	cout << "PixelX=" << mart->PixelX << endl;
	cout << "PixelY=" << mart->PixelY << endl;
	cout << "ImageWidth=" << mart->Image_Width << endl;
	cout << "ImageHeight=" << mart->Image_Height << endl;
	cout << "Itr=" << mart->NoofFullItr << endl;
	cout << "Log=" << mart->Log << endl;
	cout << "Use_Hint=" << mart->Use_Hint << endl;

	for (i = 0; i < mart->Num_Camera; i++) {
		cout << "Hint=" << mart->Hint_File[i] << endl;
	}

	cout << "Cord3D=" << mart->Cord3d_File << endl;
	cout << "Output=" << mart->Output << endl;

	for (i = 0; i < mart->Num_Camera; i++) {
		cout << "Cam=" << mart->Cameras_File[i] << endl;
	}
	for (i = 0; i < mart->Num_Camera; i++) {
		cout << "Image=" << mart->Images_File[i] << endl;
	}
	cout << "------------------------------------- \n";
	return 0;
}

int LoadConfig(const char* fileName, MART*& mart) {
	FILE* fp;
	int i;
	MartConfig mc;
	fp = fopen(fileName, "rt");
	if (!fp)
		cerr << "Can't load config file\n";
	fscanf(fp, "Num_Camera=%d;\n", &mc.Num_Camera);
	fscanf(fp, "Pixelsize=%f;\n", &mc.Pixelsize);
	fscanf(fp, "Start_VoxelX=%f;\n", &mc.Start_VoxelX);
	fscanf(fp, "End_VoxelX=%f;\n", &mc.End_VoxelX);
	fscanf(fp, "Start_VoxelY=%f;\n", &mc.Start_VoxelY);
	fscanf(fp, "End_VoxelY=%f;\n", &mc.End_VoxelY);
	fscanf(fp, "Start_VoxelZ=%f;\n", &mc.Start_VoxelZ);
	fscanf(fp, "End_VoxelZ=%f;\n", &mc.End_VoxelZ);
	fscanf(fp, "VPScaleFactor=%f;\n", &mc.VPScaleFactor);
	fscanf(fp, "Mu=%f;\n", &mc.Mu);
	fscanf(fp, "PixelX=%ld;\n", &mc.PixelX);
	fscanf(fp, "PixelY=%ld;\n", &mc.PixelY);
	fscanf(fp, "ImageWidth=%d;\n", &mc.Image_Width);
	fscanf(fp, "ImageHeight=%d;\n", &mc.Image_Height);
	fscanf(fp, "Itr=%d;\n", &mc.NoofFullItr);
	fscanf(fp, "Log=%d;\n", &mc.Log);
	fscanf(fp, "Use_Hint=%d;\n", &mc.Use_Hint);

	mart = new MART(mc.Pixelsize, mc.VPScaleFactor, mc.Start_VoxelX,
			mc.End_VoxelX, mc.Start_VoxelY, mc.End_VoxelY, mc.Start_VoxelZ,
			mc.End_VoxelZ, mc.PixelX, mc.PixelY, mc.NoofFullItr, NULL, NULL,
			NULL, NULL, NULL, NULL, 0, mc.Mu, mc.Log, mc.Use_Hint);

	for (i = 0; i < mc.Num_Camera; i++) {
		fscanf(fp, "Hint=%s\n", mart->Hint_File[i]);
	}

	fscanf(fp, "Cord3D=%s\n", mart->Cord3d_File);
	fscanf(fp, "Output=%s\n", mart->Output);
	for (i = 0; i < mc.Num_Camera; i++) {
		fscanf(fp, "Cam=%s\n", mart->Cameras_File[i]);
	}
	for (i = 0; i < mc.Num_Camera; i++) {
		fscanf(fp, "Image=%s\n", mart->Images_File[i]);
	}
	fclose(fp);

	mart->xcl = Allocate2D<double> (MAX_NUM_CALIB_MARKERS, mc.Num_Camera);
	mart->ycl = Allocate2D<double> (MAX_NUM_CALIB_MARKERS, mc.Num_Camera);
	mart->Num_Camera = mc.Num_Camera;
	mart->Image_Width = mc.Image_Width;
	mart->Image_Height = mc.Image_Height;

	return 0;
}

int LoadData(MART* mart) {
	FILE *fp1, *fp;
	int i, j, k;
	double d1, d2, d3;

	int cnt1 = 0;
	int cnt2 = 0;
	//open cordinate 3d file
	{
		if ((fp1 = fopen(mart->Cord3d_File, "rt")) == NULL) {
			cerr << "Calibration data file 1 not opened\n";
			return -1;
		}

		for (i = 1; i <= MAX_NUM_CALIB_MARKERS; i++) {
			if (fscanf(fp1, "%lf %lf %lf", &d1, &d2, &d3) == EOF)
				break;
			//TODO: take care of this.
			mart->xc3d[i] = 10 * d1; // converting to mm
			mart->yc3d[i] = 10 * d2; // converting to mm
			mart->zc3d[i] = 10 * d3; // converting to mm
			cnt1++;
		}
		fclose(fp1);
	}
	//open camera files
	{
		for (k = 0; k < mart->Num_Camera; k++) {
			if ((fp = fopen(mart->Cameras_File[k], "rt")) == NULL) {
				cerr << "Calibration data file " << mart->Cameras_File[k]
						<< " not opened" << endl;
				return -1;
			}
			cnt2 = 0;

			for (j = 1; j <= MAX_NUM_CALIB_MARKERS; j++) {
				if (fscanf(fp, "%lf %lf", &d1, &d2) == EOF)
					break;
				mart->xcl[j][k] = d1;
				mart->ycl[j][k] = d2;
				cnt2++;
			}

			if (cnt1 != cnt2) {
				cerr << "Nummber of data records different !!" << endl;
				return -1;
			}
			mart->NCP = cnt2;

			fclose(fp);
		}
	}
	//open hint files
	if(mart->Use_Hint){
		for (i = 0; i < mart->Num_Camera; i++) {
			if ((fp = fopen(mart->Hint_File[i], "rt")) == NULL) {
				cerr << "Can not load Hint files\n";
				return -1;
			}
			cnt1=0;
			while(fscanf(fp,"%lf %lf",&d1,&d2)!=EOF){
				mart->Hint[i][cnt1][0] = (uint16_t) d1;
				mart->Hint[i][cnt1][1] = (uint16_t) d2;
				cnt1++;
			}
			mart->Num_Particles[i] = cnt1;
			fclose(fp);
		}
	}

	mart->h1.xc3d = mart->xc3d;
	mart->h1.yc3d = mart->yc3d;
	mart->h1.zc3d = mart->zc3d;
	mart->h1.ncp = mart->NCP;
	mart->h1.C = mart->C;
	/*
	 for (i = 0; i < mart->NCP; i++) {
	 mart->h1.xc3d[i] = mart->xc3d[i];
	 mart->h1.yc3d[i] = mart->yc3d[i];
	 mart->h1.zc3d[i] = mart->zc3d[i];

	 for (j = 0; j <= mart->Num_Camera; j++) {
	 mart->h1.xcl[i][j] = mart->xcl[i][j];
	 mart->h1.ycl[i][j] = mart->ycl[i][j];
	 }
	 }
	 */
	return 0;
}

int LoadImage(MART* mart) {
	int i, j, k;
	int ***images;
	images = Allocate3D<int> (mart->Num_Camera, mart->Image_Width,
			mart->Image_Height);

	for (k = 0; k < mart->Num_Camera; k++) {
		FILE *fp;
		if ((fp = fopen(mart->Images_File[k], "rb")) == NULL) {
			//ShowMessage("Open file failed!");
			cerr << "Open file " << mart->Images_File[k] << " failed!";
			return -1;
		} else {
			cout << "Image " << mart->Images_File[k] << " is loaded !\n";
		}

		for (i = 0; i < mart->Image_Height; i++) {
			for (j = 0; j < mart->Image_Width; j++) {
				images[k][i][j] = (unsigned char) fgetc(fp);
				//Form1->Image1->Canvas->Pixels[j][i]=image_in[i][j]*65793;
			}
		}

		fclose(fp);
	}

	mart->Images = images;
	return 0;
}

double Timer::deltaT(int iCounterID) {
	//TODO: this file need modification to support Windows.
	double DeltaT;

	static struct timeval _NewTime; // new wall clock time (struct representation in seconds and microseconds)
	static struct timeval _OldTime0; // old wall clock time 0(struct representation in seconds and microseconds)
	static struct timeval _OldTime1; // old wall clock time 1(struct representation in seconds and microseconds)
	static struct timeval _OldTime2; // old wall clock time 2(struct representation in seconds and microseconds)

	// Get new counter reading
	gettimeofday(&_NewTime, NULL);

	switch (iCounterID) {
	case 0: {
		// Calculate time difference for timer 0.  (zero when called the first time)
		DeltaT
				= ((double) _NewTime.tv_sec + 1.0e-6
						* (double) _NewTime.tv_usec)
						- ((double) _OldTime0.tv_sec + 1.0e-6
								* (double) _OldTime0.tv_usec);

		// Reset old time 0 to new
		_OldTime0.tv_sec = _NewTime.tv_sec;
		_OldTime0.tv_usec = _NewTime.tv_usec;

		break;
	}
	case 1: {
		// Calculate time difference for timer 1.  (zero when called the first time)
		DeltaT
				= ((double) _NewTime.tv_sec + 1.0e-6
						* (double) _NewTime.tv_usec)
						- ((double) _OldTime1.tv_sec + 1.0e-6
								* (double) _OldTime1.tv_usec);

		// Reset old time 1 to new
		_OldTime1.tv_sec = _NewTime.tv_sec;
		_OldTime1.tv_usec = _NewTime.tv_usec;

		break;
	}
	case 2: {
		// Calculate time difference for timer 2.  (zero when called the first time)
		DeltaT
				= ((double) _NewTime.tv_sec + 1.0e-6
						* (double) _NewTime.tv_usec)
						- ((double) _OldTime2.tv_sec + 1.0e-6
								* (double) _OldTime2.tv_usec);

		// Reset old time 2 to new
		_OldTime2.tv_sec = _NewTime.tv_sec;
		_OldTime2.tv_usec = _NewTime.tv_usec;

		break;
	}
	default: {
		// Requested counter ID out of range
		return -9999.0;
	}
	}

	// Returns time difference in seconds sunce the last call
	return DeltaT;
}
